/*
 * sensor.c
 *
 * Created: 20/08/2011 9:08:42 a.m.
 *  Author: Isaac Manuel, Andrew Poland
 */ 
 

 
 
 
#include <avr/io.h>
#include <avr/interrupt.h>
#include "sensor.h"

#define ENABLE(x) (1<<x)
#define DISABLE(x) (1<<x)

#define GREYOFFSET 15

#define ADC_MAX 256
	//ADC_MAX - The maximum value that can be returned from the ADC



void startConversion(void);
void enableLeftAdjust(void);
void enableADCRefPin(void);
void setupSensors(void);
void setupADCStatusRegister(void);
void sensorCalibration(void);
void calibrate(void);
uint read(void);
void setGreyState(void);
void readFromSensor(Sensor *sensor);
void updateSensorReading(Sensor *currentSensor, Sensor *nextSensor);
void updateSensorColor(Sensor *currentSensor, int sensorValue);

void enableSensorInterupts(void);

#define TEST
	//Define TEST if you want to enter test mode...
	
	

//max = 0, min = 255 as this will be changed during calibration.
#ifndef TEST
	int greyStateMax = 0;
	int greyStateMin = ADC_MAX;
	volatile int calibrated = FALSE;
#endif
#ifdef TEST
	int greyStateMax = 100;
	int greyStateMin = 145;
	volatile int calibrated = TRUE;
#endif



int greyState[GREYSTATELENGTH];
	//Want to remove this after calibration?

int calibrations = 0;//Calibration iteration 


struct Sensor lSensor, rSensor;//Declare the Sensor structures.

Sensor *leftSensor = &lSensor; 
Sensor *rightSensor = &rSensor;//Declare the Sensor structures.




/*vechileState stateIdentifier[NUM_COLOURS][NUM_COLOURS] = { 
		{vechile_Left, vechile_For, vechile_Stop },
		{vechile_Left, vechile_Right, vechile_Stop},
		{vechile_Stop, vechile_Stop, vechile_Stop }
	};	*/
	
vechileState chrisTable[NUM_COLOURS][NUM_COLOURS] = 
{ 
	{vechile_Left, vechile_Left, vechile_Stop },
	{vechile_For, vechile_Right, vechile_Stop},
	{vechile_Stop, vechile_Stop, vechile_Stop }
};
/*vechileState tmpTable[NUM_COLOURS][NUM_COLOURS] = 
{ 
	{vechile_For, vechile_Rev, vechile_Stop },
	{vechile_Left, vechile_Right, vechile_Stop},
	{vechile_Stop, vechile_Stop, vechile_Stop }
};*/
//First index is the LEFT sensor value, then the RIGHT. Use as chrisTable[LeftColour][RightColour]
//Rows in the order: Black, White Grey. Columns are the same but vertical.
/*Table reads as:

L=B, R=B	L=W, R=B	L=G, R=B

L=B, R=W	L=W, R=W	L=G, R=W

L=B, R=G	L=W, R=G	L=G, R=G

*/
	

/*
 *	performs the initial setup for the sensors.
 */
void setupRead()
{
	ADMUX = 0x00;//Rest to ensure correct bits are set.
	enableLeftAdjust();
	enableADCRefPin();
	setupSensors();
	setupADCStatusRegister();
	//sensorCalibration();
	
	enableSensorInterupts();//sei();
	
}

/*
 *	returns the state the vehicle is in dependant on what the sensors
 *  are currently reading.
 */
vechileState getVehicleState()
{	
	/*int readCount = 0;
	readFromSensor(leftSensor);
	while(readCount < 2) {
		
		startConversion();
	
		int sensorValue = read();
//if leftUpdate leftSensor if right update Right Sensor.
		if(leftSensor->inUse)
		{
			updateSensorColor(leftSensor, sensorValue);
			updateSensorReading(leftSensor, rightSensor);
	
		}
		else
		{
			updateSensorColor(rightSensor, sensorValue);
			updateSensorReading(rightSensor, leftSensor);
		}
		readFromSensor(rightSensor);


	}	
	return stateIdentifier[leftSensor->currentState][rightSensor->currentState];*/
	
	
	/*TEST1 - This test block works!!*/
		//Simply go forwards or backwards depending on the binary state(Black or white) of the sensor(left) to test.
		 /*readFromSensor(leftSensor);
		 startConversion();
		 int value = read();
		 
		 if(value>128)
		 {
			return vechile_For;
		 }
		 else
		 {
			return vechile_Rev;
		}*/
	/*END - TEST1*/
	
	
	/*TEST2*/
		//Starting to incorporate more library fxns. Continues in same vain as TEST1 either For or Rev. if an unexpected case arises, it will go left.
		//This test also WORKS!!
		
		//Cant get into the FORWARDS state....
		
		
		/*Used:
			int greyStateMax = 100;
			int greyStateMin = 145;
		*/
		
		//Read from a sensor:
			unsigned int value = 128;//Mid range.
			
			
		//Get left data:	
			readFromSensor(leftSensor);
			startConversion();
			//Insert a small delay loop to wait for conversion to complete
			//Not sure if this helps, but hey it cant get much worse ;)
			volatile int i = 0;
			while(i<1000)
			{
				i++;
			}
			value = read();//The value the sensor is 'seeing'
			
			//Once read, save the value to the sensor structure:
				//updateSensorColor(leftSensor, value);
				//set if reading white.
				if(value > greyStateMax)
				{
					leftSensor->currentState = white;
				}
				//set if reading black
				else if( value < greyStateMin)
				{
					leftSensor->currentState = black;
				}
				//set if reading grey
				else 
				{
					leftSensor->currentState = grey;
				}
	
	
	
	
	
	
			
			
		//Now repeat for the RIGHT sensor:
			value = 128;
			readFromSensor(rightSensor);
			startConversion();
			i = 0;
			while(i<1000)
			{
				i++;
			}
			value = read();//The value the sensor is 'seeing'
			//updateSensorColor(rightSensor, value);
				//set if reading white.
				if(value > greyStateMax)
				{
					rightSensor->currentState = white;
				}
				//set if reading black
				else if( value < greyStateMin)
				{
					rightSensor->currentState = black;
				}
				//set if reading grey
				else 
				{
					rightSensor->currentState = grey;
				}

			
		return chrisTable[leftSensor->currentState][rightSensor->currentState];
		//return tmpTable[leftSensor->currentState][rightSensor->currentState];
		//return vechile_For;//Checked that the mothod can actually make the vechile go forward - and it CAN.
			//For some reason cant get intot the forwads state.
		 
		 
		 
		 
		 
		 
		/*if(leftSensor->currentState == black)
		{
			return vechile_For;
		}
		else if(leftSensor->currentState == white)
		{
			return vechile_Rev;
		}
		else//Must be grey.
		{
			return vechile_Left;
		}*/
	/*END - TEST2*/
}

/*
 *	returns the value of the current sensor.
 */
uint read() {
	return ADCH;
}

/*
 *	Interupt, performs all the modifications to the sensors in order
 *	to switch between sensors and read the values.
 */
ISR(ADC_vect)
{
	//Calibrate the grey area if not done already.
	if(!calibrated)
	{//Run calibration if 'calibrated' is FALSE.
		#ifndef TEST
			calibrate();
		#endif
	}
	else
	{
	
//******Do we need to call a start conversion??

		//Grab what colour a sensor is on.
		int sensorValue = read();
		
		//if leftUpdate leftSensor if right update Right Sensor.
		if(leftSensor->inUse)
		{
			updateSensorColor(leftSensor, sensorValue);
			updateSensorReading(leftSensor, rightSensor);
	
		}
		else
		{
			updateSensorColor(rightSensor, sensorValue);
			updateSensorReading(rightSensor, leftSensor);
		}
	}
}

/*
 *	Calibrates the sensors for grey input by reading grey values
 *	10 times in order to get a region defined by grey.
 */
void calibrate()
{
	if(leftSensor->inUse)
	{
		greyState[calibrations] = read();
		updateSensorReading(leftSensor, rightSensor);
		
	}
	else
	{
		greyState[calibrations] = read();
		updateSensorReading(rightSensor, leftSensor);
	}	
	calibrations++;	 
	if(calibrations >= 10)
	{
		setGreyState();
		calibrated = TRUE;	
	}
}

/*
 *	sets the calibration of the grey value by going through each
 *	measured value and finding the minimum and the maximum.
 */
void setGreyState()
{
	int i = 0;
	for(i = 0; i < GREYSTATELENGTH-1 ; i++)
	{
		if(greyState[i] < greyStateMin)
		{
			greyStateMin = greyState[i];
		}
		if(greyState[i] > greyStateMax)
		{
			greyStateMax = greyState[i];
		}
	}	
	greyStateMin -= GREYOFFSET;
	greyStateMax += GREYOFFSET;
}

/*
 *	Chooses the sensor from which to read.
 */
void readFromSensor(Sensor *sensor)
{
	switch (sensor->ioPin)
	{
	// read from left Sensor
	case 0:
		//ADMUX = ADMUX>>1;
		//ADMUX = ADMUX<<1;
		//ADMUX = ADMUX | 0x01;
		//ADMUX |= ENABLE(MUX0);
		
			ADMUX  &= ~0X01;//Clear the bit
		
		break;
	// read from right sensor
	case 1:
		//ADMUX |= DISABLE(MUX0);
		//ADMUX = ADMUX>>1;//Clear the end bit
		//ADMUX = ADMUX<<1;//But everything back into pstn
		//ADMUX = ADMUX | 0x01;//Enable the bit.
		ADMUX |= 0x01;//Set the bit

		break;
	default:
		break;
	}
}

/*
 *	changes which sensor that is being read by the ADC pin.
 */
void updateSensorReading(Sensor *currentSensor, Sensor *nextSensor)
{
	readFromSensor(nextSensor);
	currentSensor->inUse = FALSE;
	nextSensor->inUse = TRUE;
}

/*
 *	Updates the colour variable inside an object to match
 *	the most recent read.
 */
void updateSensorColor(Sensor *currentSensor, int sensorValue)
{
	//set if reading white.
	if(sensorValue > greyStateMax)
	{
		currentSensor->currentState = white;
	}
	//set if reading black
	else if( sensorValue < greyStateMin)
	{
		currentSensor->currentState = black;
	}
	//set if reading grey
	else 
	{
		currentSensor->currentState = grey;
	}
}



/*
 * method to enable left adjust reading. This allows for us to read a
 * single ADC data register ADCH and get 8 bit precision.
 */
void enableLeftAdjust()
{
	ADMUX |= ENABLE(ADLAR);
}


/* 
 *	method to enable the ADC reference to be AVcc. This alows for better 
 *	noise reduction.
 */ 
void enableADCRefPin()
{
	ADMUX |= ENABLE(REFS1);	
	ADMUX |= ENABLE(REFS0);	
}

/*	
 *	loops until the grey calibration is completed.
 */
void sensorCalibration(){
	while(!calibrated);	
}

/*
 *	sets the sensors up to read left first.
 */
void setupSensors()
{
	leftSensor->ioPin = IOPIN_LEFT;
	rightSensor->ioPin = IOPIN_RIGHT;
	leftSensor->inUse = TRUE;
	rightSensor->inUse = FALSE;
	leftSensor->currentState = black;
	rightSensor->currentState = black;
	readFromSensor(leftSensor);
}

/*
 *	Setup the ADC status registers clockcycle, interuptenabled
 *	and in free running mode.
 */
void setupADCStatusRegister()
{
	// set clock divider to 64, 12Mhz/64 = 187.5 kHz
	ADCSRA |= ENABLE(ADPS2);
	ADCSRA |= ENABLE(ADPS1);
	
	//enable interupts.
	ADCSRA |= ENABLE(ADIE);
	
	//set free running mode.
	ADCSRA |= ENABLE(ADFR);
	
	//set enabled and start conversions.
	ADCSRA |= ENABLE(ADEN);
	ADCSRA |= ENABLE(ADSC);
	
}


void startConversion()
{
	
	ADCSRA |= ENABLE(ADSC);
}


void enableSensorInterupts(void) {
	sei();
}



